# 从gazebo环境中获取无人机模型的相关信息
import rospy
import ros
from prometheus_msgs.msg import UAVControlState, UAVCommand, UAVSetup
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped
import gazebo_ros
import gazebo_plugins
import sys

class GetInfoFromGz():
    def __init__(self, uav_name):
        self.uav_name = uav_name

        self.uav_state = UAVControlState()
        self.uav_cmd = UAVCommand()
        self.uav_setup = UAVSetup()
        self.gazebo_pose = Odometry()
        rospy.Subscriber(self.uav_name + 'prometheus/state', UAVControlState, self.uav_state_callback)
        rospy.Subscriber(self.uav_name + 'prometheus/command', UAVCommand, self.uav_cmd_callback)
        rospy.Subscriber(self.uav_name + 'prometheus/setup', UAVSetup, self.uav_setup_callback)
        rospy.Subscriber(self.uav_name + 'prometheus/ground_truth', Odometry, self.gazebo_pose_callback)


    def uav_state_callback(self, msg):
        self.uav_state = msg

    def uav_cmd_callback(self, msg):
        self.uav_cmd = msg

    def uav_setup_callback(self, msg):
        self.uav_setup = msg

    def gazebo_pose_callback(self, msg):
        self.gazebo_pose = msg

    def get_uav_state(self):
        return self.uav_state

    def get_uav_cmd(self):
        return self.uav_cmd

    def get_uav_setup(self):
        return self.uav_setup

    def get_gazebo_pose(self):
        return self.gazebo_pose

if __name__ == '__main__':
    rospy.init_node('get_info_from_gz')
    param_list = rospy.get_param_names()
    # print(param_list)
    # uav_name = rospy.get_param('uav_name')
    # uav_name_space = rospy.get_param('uav_name_space')
    # print(uav_name, uav_name_space)
    get_info_from_gz = GetInfoFromGz("/uav1/")
    rate = rospy.Rate(100)
    while not rospy.is_shutdown():
        print(get_info_from_gz.get_gazebo_pose())
        rate.sleep()